#include "init.h"
#include "nutils.h"
#include "protocols.h"
#include "protocols_data.h"

#define ARRAY_ASSIGN(DEST, SRC)                                                \
  for (size_t _CNT = 0; _CNT < sizeof(SRC) / sizeof(SRC[0]); ++_CNT)           \
  {                                                                            \
    DEST[_CNT] = SRC[_CNT];                                                    \
  }

namespace linktrack
{
  AnchorFrame0_Msg g_msg_anchorframe0;
  TagFrame0_Msg g_msg_tagframe0;
  NodeFrame0_Msg g_msg_nodeframe0;
  NodeFrame1_Msg g_msg_nodeframe1;
  NodeFrame2_Msg g_msg_nodeframe2;
  NodeFrame3_Msg g_msg_nodeframe3;
  NodeFrame5_Msg g_msg_nodeframe5;
  NodeFrame6_Msg g_msg_nodeframe6;

  serial::Serial *serial_;

  Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial)
  {
    serial_ = serial;
    initAnchorFrame0(protocol_extraction);
    initTagFrame0(protocol_extraction);
    initNodeFrame0(protocol_extraction);
    initNodeFrame1(protocol_extraction);
    initNodeFrame2(protocol_extraction);
    initNodeFrame3(protocol_extraction);
    initNodeFrame5(protocol_extraction);
    initNodeFrame6(protocol_extraction);
  }

  void Init::initAnchorFrame0(NProtocolExtracter *protocol_extraction)
  {
    auto protocol = new NLT_ProtocolAnchorFrame0;
    protocol_extraction->AddProtocol(protocol);
    protocol->SetHandleDataCallback([=] {
        const auto data = nlt_anchorframe0_.result;
        g_msg_anchorframe0.role = data.role;
        g_msg_anchorframe0.id = data.id;
        g_msg_anchorframe0.voltage = data.voltage;
        g_msg_anchorframe0.local_time = data.local_time;
        g_msg_anchorframe0.system_time = data.system_time;
        auto &msg_nodes = g_msg_anchorframe0.nodes;
        msg_nodes.clear();
        decltype(g_msg_anchorframe0.nodes)::value_type msg_node;
        for (size_t i = 0, icount = data.valid_node_count; i < icount; ++i)
        {
            auto node = data.nodes[i];
            msg_node.role = node->role;
            msg_node.id = node->id;
            ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d)
            ARRAY_ASSIGN(msg_node.dis_arr, node->dis_arr)
            msg_nodes.push_back(msg_node);
        }
        g_msg_anchorframe0.show();
    });
  }

  void Init::initTagFrame0(NProtocolExtracter *protocol_extraction)
  {
    auto protocol = new NLT_ProtocolTagFrame0;
    protocol_extraction->AddProtocol(protocol);
    protocol->SetHandleDataCallback([=] {
        const auto &data = g_nlt_tagframe0.result;
        auto &msg_data = g_msg_tagframe0;

        msg_data.role = data.role;
        msg_data.id = data.id;
        msg_data.local_time = data.local_time;
        msg_data.system_time = data.system_time;
        msg_data.voltage = data.voltage;
        ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d)
        ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d)
        ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d)
        ARRAY_ASSIGN(msg_data.dis_arr, data.dis_arr)
        ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d)
        ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d)
        ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d)
        ARRAY_ASSIGN(msg_data.quaternion, data.quaternion)

        msg_data.show();
    });
  }

  void Init::initNodeFrame0(NProtocolExtracter *protocol_extraction)
  {
    auto protocol = new NLT_ProtocolNodeFrame0;
    protocol_extraction->AddProtocol(protocol);
    protocol->SetHandleDataCallback([=] {
        const auto &data = g_nlt_nodeframe0.result;
        auto &msg_data = g_msg_nodeframe0;
        auto &msg_nodes = msg_data.nodes;

        msg_data.role = data.role;
        msg_data.id = data.id;

        msg_nodes.resize(data.valid_node_count);
        for (size_t i = 0; i < data.valid_node_count; ++i)
        {
            auto &msg_node = msg_nodes[i];
            auto node = data.nodes[i];
            msg_node.id = node->id;
            msg_node.role = node->role;
            msg_node.data.resize(node->data_length);
            memcpy(msg_node.data.data(), node->data, node->data_length);
        }
        msg_data.show();
    });
  }

  void Init::initNodeFrame1(NProtocolExtracter *protocol_extraction)
  {
    auto protocol = new NLT_ProtocolNodeFrame1;
    protocol_extraction->AddProtocol(protocol);
    protocol->SetHandleDataCallback([=] {
        const auto &data = g_nlt_nodeframe1.result;
        auto &msg_data = g_msg_nodeframe1;
        auto &msg_nodes = msg_data.nodes;

        msg_data.role = data.role;
        msg_data.id = data.id;
        msg_data.local_time = data.local_time;
        msg_data.system_time = data.system_time;
        msg_data.voltage = data.voltage;

        msg_nodes.resize(data.valid_node_count);
        for (size_t i = 0; i < data.valid_node_count; ++i)
        {
            auto &msg_node = msg_nodes[i];
            auto node = data.nodes[i];
            msg_node.id = node->id;
            msg_node.role = node->role;
            ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d)
        }
        msg_data.show();
    });
  }

  void Init::initNodeFrame2(NProtocolExtracter *protocol_extraction)
  {
    auto protocol = new NLT_ProtocolNodeFrame2;
    protocol_extraction->AddProtocol(protocol);
    protocol->SetHandleDataCallback([=] {
        const auto &data = g_nlt_nodeframe2.result;
        auto &msg_data = g_msg_nodeframe2;
        auto &msg_nodes = msg_data.nodes;

        msg_data.role = data.role;
        msg_data.id = data.id;
        msg_data.local_time = data.local_time;
        msg_data.system_time = data.system_time;
        msg_data.voltage = data.voltage;
        ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d)
        ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d)
        ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d)
        ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d)
        ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d)
        ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d)
        ARRAY_ASSIGN(msg_data.quaternion, data.quaternion)

        msg_nodes.resize(data.valid_node_count);
        for (size_t i = 0; i < data.valid_node_count; ++i)
        {
            auto &msg_node = msg_nodes[i];
            auto node = data.nodes[i];
            msg_node.id = node->id;
            msg_node.role = node->role;
            msg_node.dis = node->dis;
            msg_node.fp_rssi = node->fp_rssi;
            msg_node.rx_rssi = node->rx_rssi;
        }
        msg_data.show();
    });
  }

  void Init::initNodeFrame3(NProtocolExtracter *protocol_extraction)
  {
    auto protocol = new NLT_ProtocolNodeFrame3;
    protocol_extraction->AddProtocol(protocol);
    protocol->SetHandleDataCallback([=] {
          const auto &data = g_nlt_nodeframe3.result;
          auto &msg_data = g_msg_nodeframe3;
          auto &msg_nodes = msg_data.nodes;

          msg_data.role = data.role;
          msg_data.id = data.id;
          msg_data.local_time = data.local_time;
          msg_data.system_time = data.system_time;
          msg_data.voltage = data.voltage;

          msg_nodes.resize(data.valid_node_count);
          for (size_t i = 0; i < data.valid_node_count; ++i)
          {
            auto &msg_node = msg_nodes[i];
            auto node = data.nodes[i];
            msg_node.id = node->id;
            msg_node.role = node->role;
            msg_node.dis = node->dis;
            msg_node.fp_rssi = node->fp_rssi;
            msg_node.rx_rssi = node->rx_rssi;
          }
          msg_data.show();
        });
  }

  void Init::initNodeFrame5(NProtocolExtracter *protocol_extraction)
  {
    auto protocol = new NLT_ProtocolNodeFrame5;
    protocol_extraction->AddProtocol(protocol);
    protocol->SetHandleDataCallback([=] {
        const auto &data = g_nlt_nodeframe5.result;
        auto &msg_data = g_msg_nodeframe5;
        auto &msg_nodes = msg_data.nodes;

        msg_data.role = data.role;
        msg_data.id = data.id;
        msg_data.local_time = data.local_time;
        msg_data.system_time = data.system_time;
        msg_data.voltage = data.voltage;

        msg_nodes.resize(data.valid_node_count);
        for (size_t i = 0; i < data.valid_node_count; ++i)
        {
            auto &msg_node = msg_nodes[i];
            auto node = data.nodes[i];
            msg_node.id = node->id;
            msg_node.role = node->role;
            msg_node.dis = node->dis;
            msg_node.fp_rssi = node->fp_rssi;
            msg_node.rx_rssi = node->rx_rssi;
        }
        msg_data.show();
    });
  }

  void Init::initNodeFrame6(NProtocolExtracter *protocol_extraction)
  {
    auto protocol = new NLT_ProtocolNodeFrame6;
    protocol_extraction->AddProtocol(protocol);
    protocol->SetHandleDataCallback([=] {
        const auto &data = g_nlt_nodeframe6.result;
        auto &msg_data = g_msg_nodeframe6;
        auto &msg_nodes = msg_data.nodes;

        msg_data.role = data.role;
        msg_data.id = data.id;

        msg_nodes.resize(data.valid_node_count);
        for (size_t i = 0; i < data.valid_node_count; ++i)
        {
            auto &msg_node = msg_nodes[i];
            auto node = data.nodes[i];
            msg_node.id = node->id;
            msg_node.role = node->role;
            msg_node.data.resize(node->data_length);
            memcpy(msg_node.data.data(), node->data, node->data_length);
        }

    });
  }

} // namespace linktrack
